%
%**********************************************************************

% Date: 2013-05-26  Time: 22:02:29

%************************ MODULES FOR ROCKET3 **************************
%**********************************************************************
%*** *
%*** * Calling sequence of Modules:
%*** *   G2   ENVIRONMENT
%*** *   A2   PROPULSION
%*** *   A1   AERODYNAMICS
%*** *   A3   FORCES
%*** *   C2   AUTOPILOT
%*** *   C4   ACTUATOR
%*** *   D1   NEWTONS LAW
%*** * with dummy RETURNs for unused modules
%*** *
%*** * MODIFICATION HISTORY
%*** * 000419 Version 1.0 Created by Peter Zipfel
%*** *
%**********************************************************************
%**********************************************************************

function A1I(varargin)
%**********************************************************************
%*** * Initialization of Aerodynamic Module A1.
%*** * Reserved C(3510) locations are 1200-1299
%*** * (1) Initializes placeholder index for table look-up
%*** *
%*** * MODIFICATION HISTORY
%*** * 000112 Created by Peter Zipfel
%*** *
%*** ******************************************************************

%% common c(3510);
%% common bcom_1(3510);

%*** NO INITIALIZATION CURRENTLY REQUIRED

return;
end %subroutine a1i

%**********************************************************************


%**********************************************************************
%*** * The Aerodynamic Module A1.
%*** * Reserved C(3510) locations are 1200-1299
%*** *
%*** *  MAERO =|MAERT| |MAERV|   (Type,Vehicle)
%*** *
%*** *          MAERT=1 Test Ascent Rocket
%*** *                  MAERV=1 1.Stage
%*** *                       =2 2.Stage
%*** *                       =3 3.Stage
%*** *
%*** * This module performs the following functions:
%*** * (1) Provides  CL and CD as functions of Mach
%*** *
%*** * MODIFICATION HISTORY
%*** * 000112 Created by Peter Zipfel
%*** *
%*** ******************************************************************
function A1(varargin)

	%global bcom_1; 
  global C;
  
  %if isempty(bcom_1)
  %  then bcom_1=zeros(1,3510); 
  %end;

  % Instead of referencing by value, can use a hash map by name!!!

  keys = {'MAERO'};
  values = {327.2};

  C = containers.Map(keys, values)



	%% common c(3510);
	%% common bcom_1(3510);

	%***  INPUT DATA
	maero = C('MAERO');
	alphax = C('ALPHAX');

	% equivalence(bcom_1(1200),maero) ::;
	% equivalence(bcom_1(1203),alphax) ::;

	% MAERO = D =|MAERT|MAERV|, MAERT=1:Type, MAERV:Stage #
	% ALPHAX = D/O Angle of attack - deg

	%*** INPUT FROM EXECUTIVE

	% equivalence(bcom_1(0052),crad) ::;

	crad = C('CRAD');

	%*** INPUT FROM OTHER MODULES

	vmach = C('VMACH');
	mprop = C('MPROP');

	% equivalence(bcom_1(0206),vmach) ::;
	% equivalence(bcom_1(1300),mprop) ::;

	% VMACH= O Mach number of rocket - ND
	% MPROP= D/G =0:No thrust; =1:Coasting =2:Burning

	%*** OUTPUT TO OTHER MODULES
	cd = C('CD');
	cl = C('CL');

	% equivalence(bcom_1(1201),cd) ::;
	% equivalence(bcom_1(1202),cl) ::;

	% CD = O Drag coefficient - ND
	% CL = O Lift coefficient - ND

	%*** DIAGNOSTICS
	ca = C('CA');
	cn = C('CN');
	clovercd = C('CLOVERCD');

	% equivalence(bcom_1(1206),ca) ::;
	% equivalence(bcom_1(1207),cn) ::;
	% equivalence(bcom_1(1208),clovercd) ::;

	% CA = G Axial force coefficient - ND
	% CN = G Normal force coefficient - ND
	% CLOVERCD = G Lift over drag - ND

	maert=fix(maero./10.);
	maerv=maero-maert.*10.;
	alpha=alphax./crad;
	calph=cos(alpha);
	salph=sin(alpha);


  if(maert == 1)
		if(maerv == 1)
			%*** FIRST STAGE
			if(mprop == 2)
			  caa=.281+.186.*vmach-.056.*vmach.^2+.00366.*vmach.^3;
			else;
			  caa=.346+.183.*vmach-.058.*vmach.^2+.00382.*vmach.^3;
		  end;
      cnn=(5.006-.519.*vmach+.031.*vmach.^2).*alpha;
	  end;

	  if(maerv == 2)
			%*** SECOND STAGE
			if(mprop == 2)
  			caa=.236-.043.*vmach+.0029.*vmach.^2-.00006.*vmach.^3;
			else;
	  		caa=.327-.067.*vmach+.005.*vmach.^2-.0001.*vmach.^3;
			end;
		  cnn=(1.714-.038.*vmach+.0014.*vmach.^2).*alpha;
    end;

		if(maerv == 3)
			%*** THIRD STAGE
			caa=.02;
			cnn=1.*alpha;
    end;


		%*** CONVERT COEFFICIENTS
		cdd=caa.*calph+cnn.*salph;
		cll=cnn.*calph-caa.*salph;

    C('CL')=cll;
		C('CD')=cdd;

		C('CLOVERCD')=cl./cd;

		C('CN')=cl.*calph+cd.*salph;
		C('CA')=cd.*calph-cl.*salph;

	end;

	return;
end %subroutine A1

%**********************************************************************


%**********************************************************************
%*** * Propulsion Initialization.
%*** * Reserved C(3510) locations are 1300-1399
%*** *
%*** * * Fixed nozzle, variable fuel flow motor, ideal nozzle expansion
%*** * is assumed. Throttle ratio THRTL
%*** * governs the flow rate.
%*** * * Several stages; the total mass of the remaining stages is initialized
%*** * by VMASSI; new engine parameters may also be introduced at every
%*** * stage; the current mass is VMASS;
%*** *
%*** * MPROP =0 No ignition; or engine shut-down (D)
%*** *       =1 Missile is coasting after emgine shut-down.
%*** *          (shut-down occurs either at fuel burn-out or
%*** *           when MPROP=0 set in input (G)
%*** *       =2 Motor ignition; re-ignition; or motor burning (D)
%*** *
%*** * This module performs the following functions:
%*** *
%*** * (1) Initializes vehicle and fuel masses
%*** *
%*** * MODIFICATION HISTORY
%*** * 000113 Created by Peter Zipfel
%*** *
%*** ******************************************************************
function A2I(varargin)

  %*** INPUT DATA
  % VMASSI = D Initial mass of remaining stages - kg
  vmassi = C('VMASSI');

  %*** INITIALIZATION
  % FMASSF = I/G Expended fuel in stage - kg
  % VMASS = I/O Vehicle mass - kg
  % FMASSFM = I Expended fuel in stage, stored - kg
  % VMASSIM = I Initial mass of prior stage - kg

  C('FMASSF')=0.;
  C('VMASS')=vmassi;
  C('FMASSFM')=0.;
  C('VMASSIM')=0.;

  return;
end %subroutine a2i
%**********************************************************************


%**********************************************************************
%*** * Propulsion.
%*** * Reserved C(3510) locations are 1300-1399
%*** *
%*** * This module performs the following functions:
%*** *
%*** * (1) Calculates missile thrust
%*** * (2) Initiates rocket booster staging
%*** * (3) Calculates missile mass
%*** *
%*** * MODIFICATION HISTORY
%*** * 000113 Created by Peter Zipfel
%*** *
%*** ******************************************************************
function A2(varargin)

  %*** INPUT DATA      
  % MPROP = D/G =0:No thrust; =1:Coasting =2:Burning
  % FUELI = D Initial total fuel in stage - kg
  % FUELR = D Maximum fuel rate through motor - kg/s
  % SPI = D Effective specific impulse - s
  % VMASSI = D Initial mass of remaining stages - kg
  % THRTL = D Throttle (0-1) - ND
  % AEXIT = D Nozzle exit aerea - m^2
  % PEXIT = D Rocket nozzle exit pressure - Pa

  mprop = C('MPROP');
  fueli = C('FUELI');
  fuelr = C('FUELR');
  spi = C('SPI');
  vmassi = C('VMASSI');
  thrtl = C('THRTL');
  aexit = C('AEXIT');
  pexit = C('PEXIT');


  %*** INIIALIZATION
  % FMASSF = I/G Expended fuel in stage - kg
  % FMASSFM = I Expended fuel in stage, stored - kg
  % VMASSIM = I Initial mass of prior stage - kg

  fmassf = C('FMASSF');
  fmassfm = C('FMASSFM');
  vmassim = C('VMASSIM');

  %*** INPUT FROM EXECUTIVE
  % AGRAV = E Gravitational acceleration (refrence) - m/s^2

  agrav = C('AGRAV');
  der = C('DER');
  icoor = C('ICOOR');

  %*** INPUT FROM OTHER MODULES
  % PATM= O Atmospheric pressure - Pa
  patm = C('PATM');

  %*** OUTPUT TO OTHER MODULES

  % equivalence(bcom_1(1313),thrustx) ::;
  % equivalence(bcom_1(1309),vmass) ::;

  % THRUSTX = O Thrust at altitude - kN
  % VMASS = O Vehicle mass - kg

  %*** DIAGNOSTICS

  % equivalence(bcom_1(1306),fmassfd) ::;
  % equivalence(bcom_1(1311),fuel) ::;

  % FMASSFD = G Fuel rate - kg/s
  % FUEL = G Fuel remaining in stage - kg

  %*** FOR NEW STAGE: INITIALIZE EXPENDED FUEL TO ZERO

  if((vmassim-vmassi) > 0.)
    fmassfm=0.;
    fmassf=0.;
    fmassfd=0.;
  end;

  vmassim=vmassi;

  %*** CALCULATE FUEL EXPENDED IN STAGE

  if(icoor == 0)
    fmassf=fmassf+fmassfd.*der;
  end;

  if(mprop == 0)
    %*** NO BURNING
    thrust=0.;
    fmassfd=0.;
    vmass=vmassi-fmassfm;
    fuel=fueli-fmassfm;
  end;

  if(mprop == 1)
    %*** COAST AFTER BURN-OUT
    thrust=0.;
    fmassfd=0.;
    fmassf=0.;
    vmass=vmassi-fmassfm;
    fuel=fueli-fmassfm;
  end;

  if(mprop == 2)
    %*** ROCKET MOTOR BURNING

    flr=fuelr.*thrtl;

    %-- Change this to use the Pe variable
    %         THRUST=FLR*SPI*AGRAV+(101325.-PATM)*AEXIT
    thrust=flr.*spi.*agrav+(pexit-patm).*aexit;
    %         PRINT *, 'T to W:', THRUST / VMASS
    fmassfd=flr;
    fuel=fueli-fmassf;
    vmass=vmassi-fmassf;
    fmassfm=fmassf;
    if(fuel <= 0.)
      mprop=1;
      thrust=0.;
      fmassfd=0.;
      fmassf=0.;
    end;
  end;

  thrustx=thrust./1000.;

  %*** OUTPUT TO OTHER MODULES  
  C('THRUSTX') = thrustx;
  C('VMASS') = vmass;
  C('FUEL') = fuel;
  C('FMASSFD') = fmassfd;

  return;
end %subroutine A2
%**********************************************************************


%**********************************************************************
%*** * Force Module A3
%*** * Reserved C(3510) locations are 1400-1499
%*** * This module performs the following functions:
%*** *
%*** * Calculates the specific force acting on the vehicle
%*** *
%*** * MODIFICATION HISTORY
%*** * 960701  Created by Peter Zipfel
%*** *
%*** ******************************************************************

function A3(varargin)

  persistent fapm fspv ; 

  if isempty(fapm), fapm=zeros(1,3); end;
  if isempty(fspv), fspv=zeros(1,3); end;

  %*** INPUT DATA
  % SREF = D Aerodynamic reference area - m^2
  % PHIMVX = D Bank angle of maneuver plane wrt vertical - deg

  sref = C('SREF');
  phimvx = C('PHIMVX');

  %*** INPUT FROM EXECUTIVE
  crad = C('CRAD');

  %*** INPUT FROM OTHER MODULES
  % PDYNMC= G Dynamic Pressure - Pa
  % CD= O Drag coefficient - ND
  % CL= O Lift coefficient - ND
  % ALPHAX= D/O Angle of attack - deg
  % VMASS= I/O Vehicle mass - kg
  % THRUSTX= O Thrust at altitude - kN
  % DVBE= I/G Geographic speed - m/s  

  pdynmc = C('PDYNMC');
  cd = C('CD');
  cl = C('CL');
  alphax = C('ALPHAX');
  vmass = C('VMASS');
  thrustx = C('THRUSTX');
  dvbe = C('DVBE');  

  %*** OUTPUTS TO OTHER MODULES

  % equivalence(bcom_1(1405),fspv(1)) ::;

  % FSPV(3) = O Specific force in velocity coordinates - m/s^2

  %*** DIAGNOSTICS

  % equivalence(bcom_1(1403),fd) ::;
  % equivalence(bcom_1(1404),fl) ::;

  % FD = G Drag force on vehicle - N
  % FL = G Lift force on vehicle - N
  % PDYNMC = G Dynamic Pressure - Pa

  %*** CALCULATE AERODYNAMIC FORCES

  fd=pdynmc.*sref.*cd;
  fl=pdynmc.*sref.*cl;

  %*** CALCULATE NON-GRAVITATIONAL FORCES IN MANEUVER PLANE

  fapm(1)=-fd+thrustx.*1000..*cos(alphax./crad);
  fapm(2)=0.;
  fapm(3)=-(fl+thrustx.*1000..*sin(alphax./crad));

  %*** SPECIFIC FORCE IN VELOCITY AXES

  fspv(1)=fapm(1)./vmass;
  fspv(2)=-sin(phimvx./crad).*fapm(3)./vmass;
  fspv(3)=cos(phimvx./crad).*fapm(3)./vmass;


  % Can store the vectors in a hash???
  % Otherwise create a global variable for all vectors and matrices

  C('FSPV') = fspv;
  C('FAPM') = fapm;

  return;
end %subroutine a3
%**********************************************************************


%**********************************************************************
%*** *
%*** * Atmosphere and gravity module in SI units
%*** * Reserved C(3510) locations are 200-299
%*** * This module performs the following functions:
%*** * 1) Calculates the atmospheric properties
%*** * 2) Calculates the gravitational acceleration
%*** * 3) Calculates the vehicle Mach number and dynamic pressure
%*** * 4) Inputs special weather deck from INPUT.ASC
%*** *
%*** * MAIR=0 International Standard Atmosphere ISO 1962
%*** *     =1 Weather Deck (Atmophere only, wind not used in 3 DoF sims)
%*** * COMMOM /WINDS/ read in from INPUT.ASC WEATHER deck
%*** *  (OPTMET=1 required, SI units)
%*** *  WALT= Altitude - m
%*** *  WDIR= Wind Direction (from North) - deg
%*** *  WVEL= Wind Velocity - m/s
%*** *  RHX= Air density - kg/m^3
%*** *  CTMP= Temprature - deg Celsius
%*** *  WPRES= Atmospheric pressure - Pa
%*** *  KOUNTW= Number of altitude records
%*** *  RHW= Last altitude record
%*** *
%*** * MODIFICATION HISTORY
%*** * 931007 Created by Peter Zipfel
%*** *
%*** ******************************************************************
function G2(varargin)

  persistent emass g r ; 
  balt=[];rho=[];ctemp=[];patm=[];

  global winds_1; if isempty(winds_1), winds_1=zeros(1,50); end;
  global winds_2; if isempty(winds_2), winds_2=zeros(1,50); end;
  global winds_3; if isempty(winds_3), winds_3=zeros(1,50); end;
  global winds_4; if isempty(winds_4), winds_4=zeros(1,50); end;
  global winds_5; if isempty(winds_5), winds_5=zeros(1,50); end;
  global winds_6; if isempty(winds_6), winds_6=zeros(1,50); end;
  global winds_7; if isempty(winds_7), winds_7=0; end;
  global winds_8; if isempty(winds_8), winds_8=0; end;
  %% common /winds/walt(50),wdir(50),wvel(50),rhx(50),ctmp(50),wpres(50),kountw,rhw;
  %% common /winds/winds_1(50),winds_2(50),winds_3(50),winds_4(50),winds_5(50),winds_6(50),winds_7,winds_8;

  %*** INPUT DATA

  % equivalence(bcom_1(0200),mair) ::;

  % MAIR = D =0:Std Atmosphere, =1: Weather Deck

  %*** INPUT FROM EXECUTIVE ROUTINE

  % equivalence(bcom_1(0051),rearth) ::;

  % REARTH = E Radius of Earth - m

  %*** INPUT FROM OTHER MODULES

  % equivalence(bcom_1(1606),balt) ::;
  % equivalence(bcom_1(1613),dvbe) ::;

  % BALT= I/O Vehicle altitude = m
  % DVBE= I/G Geographic speed - m/s

  %*** OUTPUT TO OTHER MODULES

  % equivalence(bcom_1(0202),patm) ::;
  % equivalence(bcom_1(0203),rho) ::;
  % equivalence(bcom_1(0205),grav) ::;
  % equivalence(bcom_1(0206),vmach) ::;
  % equivalence(bcom_1(0207),pdynmc) ::;

  % PATM = O Atmospheric pressure - Pa
  % RHO = O Atmospheric density - kg/m^3
  % GRAV = O Gravity acceleration - m/s^2
  % VMACH = O Mach number of rocket - ND
  % PDYNMC = O Dynamic pressure - Pa

  %*** DIAGNOSTICS

  % equivalence(bcom_1(0201),tempk) ::;
  % equivalence(bcom_1(0204),vsound) ::;

  % TEMPK = G Atmospheric temperature - degK
  % VSOUND = G Sonic speed - m/sec


  %*** PARAMETERS

  if isempty(g), g=6.673e-11 ; end;
  if isempty(r), r=287.053 ; end;
  if isempty(emass), emass=5.973e24 ; end;

  % G =Gravitaional constant - N*m^2/kg^2
  % R =Gas constant - m^2/(K*sec^2
  % EMASS =Mass of earth - kg

  %*** ALTITUDE ABOVE CENTER OF EARTH

  rad = rearth + balt;

  %*** CALCULATE THE GRAVITY ACCELERATION

  grav=g.*emass./rad.^2;

  %*** CALCUL THE ATMOSPH DENSITY, SONIC SPEED AND ROCKET MACH NUMBER

  if(mair == 0)
  if(balt < 11000.)
  tempk=288.15-0.0065.*balt;
  patm=101325..*(tempk./288.15).^5.2559;
  else;
  tempk=216.;
  patm=22630..*exp(-0.00015769.*(balt-11000.));
  end;

  rho=patm./(r.*tempk);
  else;
  [balt,winds_1,winds_4,winds_7,rho]=table(balt,winds_1,winds_4,winds_7,rho);
  [balt,winds_1,winds_5,winds_7,ctemp]=table(balt,winds_1,winds_5,winds_7,ctemp);
  [balt,winds_1,winds_6,winds_7,patm]=table(balt,winds_1,winds_6,winds_7,patm);
  tempk=ctemp+273.16;
  end;

  vsound=sqrt(1.4.*r.*tempk);

  vmach=abs(dvbe./vsound);

  pdynmc=rho.*dvbe.^2./2.;

  return;
end %subroutine g2
%**********************************************************************


%*** ******************************************************************
%*** * Initializes the equations of motions of Module D1
%*** * Reserved C(3510) locations are 1600-1699
%*** * This module performs the following functions
%*** *
%*** * 1) Define the locations of the state and state derivative
%*** *    variables
%*** * 2) Converts geographic inputs into inertial coordinates
%*** *
%*** * MODIFICATION HISTORY
%*** * 960711 Created by Peter Zipfel
%*** *
%*** ******************************************************************
function D1I(varargin)

  persistent dum3 ipl iplv sbie sbii teg tge tgv tie tig tvg vbeg vbei vbii weii ; 
  dvbe=[];psivg=[];thtvg=[];blon=[];blat=[];


  if isempty(ipl), ipl=zeros(1,100); end;
  if isempty(iplv), iplv=zeros(1,100); end;
  if isempty(vbeg), vbeg=zeros(1,3); end;
  if isempty(tge), tge=zeros(3,3); end;
  if isempty(teg), teg=zeros(3,3); end;
  if isempty(sbie), sbie=zeros(1,3); end;
  if isempty(tvg), tvg=zeros(3,3); end;
  if isempty(tgv), tgv=zeros(3,3); end;
  if isempty(tie), tie=zeros(3,3); end;
  if isempty(sbii), sbii=zeros(1,3); end;
  if isempty(tig), tig=zeros(3,3); end;
  if isempty(weii), weii=zeros(3,3); end;
  if isempty(dum3), dum3=zeros(1,3); end;
  if isempty(vbei), vbei=zeros(1,3); end;
  if isempty(vbii), vbii=zeros(1,3); end;

  %*** INPUT DATA INITIALIZATION

  % equivalence(bcom_1(1602),psivgx) ::;
  % equivalence(bcom_1(1603),thtvgx) ::;
  % equivalence(bcom_1(1604),blon) ::;
  % equivalence(bcom_1(1605),blat) ::;
  % equivalence(bcom_1(1606),balt) ::;
  % equivalence(bcom_1(1610),baltft) ::;
  % equivalence(bcom_1(1613),dvbe) ::;

  % PSIVGX = I Heading angle from north - deg
  % THTVGX = I Flight path angle from horizontal - deg
  % BLON = I/G Vehicle longitude - rad
  % BLAT = I/G Vehicle latitude - rad
  % BALT = I/O Vehicle altitude - m
  % BALTFT = I/O Vehicle altitude - ft
  % DVBE = I/G Geographic speed - m/s

  %*** INPUT FROM EXECUTIVE

  % equivalence(bcom_1(0051),rearth) ::;
  % equivalence(bcom_1(0052),crad) ::;
  % equivalence(bcom_1(0058),weii3) ::;
  % equivalence(bcom_1(2562),ipl(1)) ::;
  % equivalence(bcom_1(2867),iplv(1)) ::;
  % equivalence(bcom_1(2561),nip) ::;

  % IPL(100) = E State derivitave bcom_1-array locations
  % IPLV(100) = E State bcom_1-array locations
  % N = E Number of variables to integrate

  %*** INITIALIZATION

  % equivalence(bcom_1(1622),tgv(1,1)) ::;
  % equivalence(bcom_1(1631),tig(1,1)) ::;
  % equivalence(bcom_1(1649),sbii(1)) ::;
  % equivalence(bcom_1(1643),vbii(1)) ::;
  % equivalence(bcom_1(1658),balt0) ::;

  %***  INITIALIZATION OF STATE VARIABLES

  iloc=1640;
  for i=0:2;
  ipl(nip)=fix(iloc+i);
  iplv(nip)=fix(iloc+i+3);
  nip=nip+1;
  end; i=2+1;

  iloc=1646;
  for i=0:2;
  ipl(nip)=fix(iloc+i);
  iplv(nip)=fix(iloc+i+3);
  nip=nip+1;
  end; i=2+1;

  %***INPUT CONVERSION TO SBII AND VBII AND INITIAL TGV AND TIG

  sbie(1)=(balt+rearth).*cos(blat).*cos(blon);
  sbie(2)=(balt+rearth).*cos(blat).*sin(blon);
  sbie(3)=(balt+rearth).*sin(blat);
  [tie]=matuni(tie,3);
  sbii*tie,sbie,3,3,1);

  psivg=psivgx./crad;
  thtvg=thtvgx./crad;
  [vbeg,dvbe,psivg,thtvg]=matcar(vbeg,dvbe,psivg,thtvg);
  [tge,blon,blat]=cadtge3(tge,blon,blat);
  [teg,tge]=mattra(teg,tge,3,3);
  [weii]=matzer(weii,3,3);
  weii(1,2)=-weii3;
  weii(2,1)=weii3;
  dum3*weii,sbii,3,3,1);
  tig*tie,teg,3,3,3);
  vbei*tig,vbeg,3,3,1);
  [vbii,vbei,dum3]=matadd(vbii,vbei,dum3,3,1);
  [tvg,psivg,thtvg]=mat2tr(tvg,psivg,thtvg);
  [tgv,tvg]=mattra(tgv,tvg,3,3);

  %*** SAVE LAUNCH ALTITUDE

  balt0=balt;
  baltft=balt.*3.2808399;

  return;
end %subroutine d1i
%*******************************************************************


%*******************************************************************
%*** * Equations of motion Module D1
%*** * Cartesian inertial form, round rotating earth
%*** * Reserved C(3510) locations are 1600-1699
%*** * This module performs the following functions
%*** *
%*** * 1) Solves Newton's Law for spherical rotating earth in
%*** *    inertial coordinates
%*** * 2) Converts output to geographic variables
%*** *
%*** * MODIFICATION HISTORY
%*** * 960711 Created by Peter Zipfel
%*** *
%*** **************************************************************
function D1(varargin)

  persistent accg agravg ai dum3 fspg fspv sbie sbii sbiid tei tge tgi tgv tig tvg vbeg vbei vbig vbii vbiid weii ; 
  grav=[];blon=[];blat=[];balt=[];dbi=[];dvbe=[];psivg=[];thtvg=[];dvbi=[];psivig=[];thtvig=[];
  global bcom_1; if isempty(bcom_1), bcom_1=zeros(1,3510); end;
  %% common c(3510);
  %% common bcom_1(3510);

  if isempty(fspg), fspg=zeros(1,3); end;
  if isempty(fspv), fspv=zeros(1,3); end;
  if isempty(agravg), agravg=zeros(1,3); end;
  if isempty(ai), ai=zeros(1,3); end;
  if isempty(tig), tig=zeros(3,3); end;
  if isempty(tei), tei=zeros(3,3); end;
  if isempty(sbie), sbie=zeros(1,3); end;
  if isempty(sbii), sbii=zeros(1,3); end;
  if isempty(vbei), vbei=zeros(1,3); end;
  if isempty(tge), tge=zeros(3,3); end;
  if isempty(tgi), tgi=zeros(3,3); end;
  if isempty(tvg), tvg=zeros(3,3); end;
  if isempty(tgv), tgv=zeros(3,3); end;
  if isempty(weii), weii=zeros(3,3); end;
  if isempty(vbig), vbig=zeros(1,3); end;
  if isempty(vbii), vbii=zeros(1,3); end;
  if isempty(vbiid), vbiid=zeros(1,3); end;
  if isempty(sbiid), sbiid=zeros(1,3); end;
  if isempty(dum3), dum3=zeros(1,3); end;
  if isempty(vbeg), vbeg=zeros(1,3); end;
  if isempty(accg), accg=zeros(1,3); end;

  %*** INPUT FROM EXECUTIVE

  % equivalence(bcom_1(0051),rearth) ::;
  % equivalence(bcom_1(0052),crad) ::;
  % equivalence(bcom_1(0058),weii3) ::;
  % equivalence(bcom_1(2000),t) ::;

  % CRAD = E Conversion from radians to degrees = 57.298
  % WEII3 = E Earth rotation - rad/sec

  %*** INITIALIZATION

  % equivalence(bcom_1(1622),tgv(1,1)) ::;
  % equivalence(bcom_1(1631),tig(1,1)) ::;
  % equivalence(bcom_1(1658),balt0) ::;

  % TGV(3,3) = I T.M. of  geographic wrt velocity coord - ND
  % TIG(3,3) = I T.M. of inertial wrt geographic coord - ND
  % BALT0 = I Saved value of initial altitude - m

  %***  INPUT FROM OTHER MODULES

  % equivalence(bcom_1(0205),grav) ::;
  % equivalence(bcom_1(1405),fspv(1)) ::;

  % GRAV= O Gravity acceleration - m/s^2
  % FSPV= O Specific force in velocity coordinates - m/s^2

  %*** STATE VARIABLES

  % equivalence(bcom_1(1640),vbiid(1)) ::;
  % equivalence(bcom_1(1643),vbii(1)) ::;
  % equivalence(bcom_1(1646),sbiid(1)) ::;
  % equivalence(bcom_1(1649),sbii(1)) ::;

  % VBIID(3) = S Time derivative of VBII(3) - m/s^2
  % VBII(3) = S Vel of missile wrt inertial frame in inertial axes - m
  % SBIID(3) = S Time derivative of SBIE(3) - m/s
  % SBII(3) = S Missile displacement from earth center in inertial axes - m

  %*** OUTPUT TO OTHER MODULES

  % equivalence(bcom_1(1606),balt) ::;
  % equivalence(bcom_1(1613),dvbe) ::;

  % BALT = O Vehicle altitude = m
  % DVBE = I/O Geographic speed - m/s

  %*** DIAGNOSTICS

  % equivalence(bcom_1(1602),psivgx) ::;
  % equivalence(bcom_1(1603),thtvgx) ::;
  % equivalence(bcom_1(1604),blon) ::;
  % equivalence(bcom_1(1605),blat) ::;
  % equivalence(bcom_1(1607),dvbi) ::;
  % equivalence(bcom_1(1608),psivigx) ::;
  % equivalence(bcom_1(1609),thtvigx) ::;
  % equivalence(bcom_1(1610),baltft) ::;
  % equivalence(bcom_1(1652),vbeg(1)) ::;
  % equivalence(bcom_1(1655),vbig(1)) ::;

  % PSIVGX = G Heading angle from north - deg
  % THTVGX = G Flight path angle from horizontal - deg
  % BLON = G Vehicle longitude - rad
  % BLAT = G Vehicle latitude - rad
  % DVBI = G Speed of vehicle wrt inertial frame
  % PSIVIGX = G Heading angle of inertial vel vect - deg
  % THTVIGX = G Flight path angle of inert vel vec  - deg
  % BALTFT = G Vehicle Altitude - ft
  % VBEG(3) = G Geographic velocity in geographic coord - m/s
  % VBIG(3) = G Inertial velocity in geographic coord - m/s

  %*** RIGHT HAND SIDE OF DYNAMIC EQUATIONS

  fspg*tgv,fspv,3,3,1);

  [agravg,dumvar2,dumvar3,grav]=vecvec(agravg,0.,0.,grav);

  [accg,fspg,agravg]=matadd(accg,fspg,agravg,3,1);

  [dum3,accg]=mateql(dum3,accg,3,1);

  ai*tig,dum3,3,3,1);

  %*** STATE VARIABLE INTEGRATION

  [vbiid,ai]=mateql(vbiid,ai,3,1);
  [sbiid,vbii]=mateql(sbiid,vbii,3,1);

  %*** UPDATE LONGITUDE, LATITUDE AND ALTITUDE, TVG AND FLIGHT PATH ANGLES

  [tei]=cadtei3(tei);
  sbie*tei,sbii,3,3,1);
  [blon,blat,balt,dbi,sbie]=cadsph3(blon,blat,balt,dbi,sbie);
  [tge,blon,blat]=cadtge3(tge,blon,blat);

  [weii]=matzer(weii,3,3);
  weii(1,2)=-weii3;
  weii(2,1)=weii3;
  dum3*weii,sbii,3,3,1);
  [vbei,vbii,dum3]=matsub(vbei,vbii,dum3,3,1);
  tgi*tge,tei,3,3,3);
  vbeg*tgi,vbei,3,3,1);
  [dvbe,psivg,thtvg,vbeg]=matpol(dvbe,psivg,thtvg,vbeg);
  psivgx=psivg.*crad;
  thtvgx=thtvg.*crad;

  %*** FOR NEXT INTEGRATION CYCLE: TIG, TGV

  [tig,tgi]=mattra(tig,tgi,3,3);
  [tvg,psivg,thtvg]=mat2tr(tvg,psivg,thtvg);
  [tgv,tvg]=mattra(tgv,tvg,3,3);

  %*** DIAGNOSTIC: INERTIAL VELOCITY IN GEOGRAPHIC AXES

  vbig*tgi,vbii,3,3,1);
  [dvbi,psivig,thtvig,vbig]=matpol(dvbi,psivig,thtvig,vbig);
  psivigx=psivig.*crad;
  thtvigx=thtvig.*crad;
  %---
  %-- Is this the right spot???
  %-- Save the altitude in feet.
  baltft=balt.*3.2808399;

  return;
end %subroutine d1

%*******************************************************************


%*** ***************************************************************
%*** * Calculates longitude, latitude and altitude from earth position
%*** *
%*** * Argument Output:
%*** *          BLON =Missile longitude - rad
%*** *          BLAT =Missile latitude - rad
%*** *          BALT =Missile altitude above sea level - rad
%*** *          DBI =Missile distance from earth center - m
%*** * Argument Input:
%*** *          SBIE(3) =Missile position wrt earth center in earth coor - m
%*** *
%*** * MODIFICATION HISTORY
%*** * 960628 Created by Peter Zipfel
%*** * 000128 Resolved multivalued ARCSIN function, PZi
%*** *
%*** ***************************************************************
function [blon,blat,balt,dbi,sbie]=cadsph3(blon,blat,balt,dbi,sbie);

  global bcom_1; if isempty(bcom_1), bcom_1=zeros(1,3510); end;
  %% common  c(3510);
  %% common  bcom_1(3510);



  %*** INPUT FROM EXEC

  % equivalence(bcom_1(0051),rearth) ::;
  % equivalence(bcom_1(0052),crad) ::;

  %*** LATITUDE

  dbi=sqrt(sbie(1).^2+sbie(2).^2+sbie(3).^2);
  dum1=sbie(3)./dbi;
  if(abs(dum1) > 1.)
  dum1=(abs(1.).*sign(dum1));
  end;
  blat=asin(dum1);

  %*** ALTITUDE

  balt=dbi-rearth;
  baltft=balt.*3.2808399;

  %*** LONGITUDE

  dum3=sbie(2)./sqrt(sbie(1).^2+sbie(2).^2);
  if(abs(dum3) > 1.)
  dum3=(abs(1.).*sign(dum3));
  end;
  dum4=asin(dum3);

  %*** RESOLVING THE MUTLIVALUED ARCSIN FUNCTION

  %1. quadrant
  if(sbie(1) >= 0.&&sbie(2) >= 0.)
  alamda=dum4;
  end;
  %2. quadrant
  if(sbie(1) < 0.&&sbie(2) >= 0.)
  alamda=180../crad-dum4;
  end;
  %3. quadrant
  if(sbie(1) < 0.&&sbie(2) < 0.)
  alamda=180../crad-dum4;
  end;
  %4. quadrant
  if(sbie(1) >= 0.&&sbie(2) < 0.)
  alamda=360../crad+dum4;
  end;
  blon=alamda;
  %east pos., west neg.
  if(blon >(180../crad))
  blon=-((360../crad)-blon);
  end;

  return;
end %subroutine cadsph3
%*******************************************************************


%*** ***************************************************************
%*** * Calculates transformation matrix TGE from longitude and latitude
%*** *
%*** * Argument Output:
%*** *          TGE(3,3) =Transf. of geographic wrt earth coor
%*** * Argument Input:
%*** *          BLON =Missile longitude - rad
%*** *          BLAT =Missile latitude - rad
%*** *
%*** * MODIFICATION HISTORY
%*** * 960628 Created by Peter Zipfel
%*** *
%*** ***************************************************************
function [tge,blon,blat]=cadtge3(tge,blon,blat);


  tge_orig=tge;tge_shape=[3,3];tge=reshape([tge_orig(1:min(prod(tge_shape),numel(tge_orig))),zeros(1,max(0,prod(tge_shape)-numel(tge_orig)))],tge_shape);
  global bcom_1; if isempty(bcom_1), bcom_1=zeros(1,3510); end;
  %% common  c(3510);
  %% common  bcom_1(3510);

  slon=sin(blon);
  clon=cos(blon);
  slat=sin(blat);
  clat=cos(blat);
  tge(1,1)=-slat.*clon;
  tge(1,2)=-slat.*slon;
  tge(1,3)=clat;
  tge(2,1)=-slon;
  tge(2,2)=clon;
  tge(2,3)=0.;
  tge(3,1)=-clat.*clon;
  tge(3,2)=-clat.*slon;
  tge(3,3)=-slat;

  tge_orig(1:min(prod(tge_shape),numel(tge_orig)))=tge(1:min(prod(tge_shape),numel(tge_orig)));tge=tge_orig;
  return;

end %subroutine cadtge3
%*******************************************************************


%*** ***************************************************************
%*** * Calculates transformation matrix TIE from time and WEII3
%*** *
%*** * Argument Output:
%*** *          TEI(3,3) =Transf. of inertial wrt earth coor
%*** *
%*** * MODIFICATION HISTORY
%*** * 960711 Created by Peter Zipfel
%*** *
%*** ***************************************************************
function [tei]=cadtei3(tei);


  tei_orig=tei;tei_shape=[3,3];tei=reshape([tei_orig(1:min(prod(tei_shape),numel(tei_orig))),zeros(1,max(0,prod(tei_shape)-numel(tei_orig)))],tei_shape);
  global bcom_1; if isempty(bcom_1), bcom_1=zeros(1,3510); end;
  %% common  c(3510);
  %% common  bcom_1(3510);



  %*** INPUT FROM EXECUTIVE

  % equivalence(bcom_1(0058),weii3) ::;
  % equivalence(bcom_1(2000),t) ::;

  xi=weii3.*t;
  sxi=sin(xi);
  cxi=cos(xi);
  [tei]=matuni(tei,3);
  tei(1,1)=cxi;
  tei(1,2)=sxi;
  tei(2,1)=-sxi;
  tei(2,2)=cxi;

  tei_orig(1:min(prod(tei_shape),numel(tei_orig)))=tei(1:min(prod(tei_shape),numel(tei_orig)));tei=tei_orig;
  return;

end %subroutine cadtei3
%********************* DUMMY RETURNS **********************************

function a3i(varargin)

return;
end %subroutine a3i

function a4i(varargin)

return;
end %subroutine a4i

function a4(varargin)

return;
end %subroutine a4

function a5i(varargin)

return;
end %subroutine a5i

function a5(varargin)

return;
end %subroutine a5

function c1i(varargin)

return;
end %subroutine c1i

function c1(varargin)

return;
end %subroutine c1
%      SUBROUTINE C2I
%      RETURN
%      END
%      SUBROUTINE C2
%      RETURN
%      END

function c3i(varargin)

return;
end %subroutine c3i

function c3(varargin)

return;
end %subroutine c3
%       SUBROUTINE C4I
%      RETURN
%      END
%      SUBROUTINE C4
%      RETURN
%      END

function c5i(varargin)

return;
end %subroutine c5i

function c5(varargin)

return;
end %subroutine c5

function d2i(varargin)

return;
end %subroutine d2i

function d2(varargin)

return;
end %subroutine d2

function d3i(varargin)

return;
end %subroutine d3i

function d3(varargin)

return;
end %subroutine d3

function d4(varargin)

return;
end %subroutine d4

function d4i(varargin)

return;
end %subroutine d4i

function d5i(varargin)

return;
end %subroutine d5i

function d5(varargin)

return;
end %subroutine d5

function g1i(varargin)

return;
end %subroutine g1i

function g1(varargin)

return;
end %subroutine g1

function g2i(varargin)

return;
end %subroutine g2i

function g3i(varargin)

return;
end %subroutine g3i

function g3(varargin)

return;
end %subroutine g3

function g4i(varargin)

return;
end %subroutine g4i

function g4(varargin)

return;
end %subroutine g4

function g5i(varargin)

return;
end %subroutine g5i

function g5(varargin)

return;
end %subroutine g5

function s1i(varargin)

return;
end %subroutine s1i

function s1(varargin)

return;
end %subroutine s1

function s2i(varargin)

return;
end %subroutine s2i

function s2(varargin)

return;
end %subroutine s2

function s3i(varargin)

return;
end %subroutine s3i

function s3(varargin)

return;
end %subroutine s3

function s4i(varargin)

return;
end %subroutine s4i

function s4(varargin)

return;
end %subroutine s4

function s5i(varargin)

return;
end %subroutine s5i

function s5(varargin)

return;
end %subroutine s5
%**********************************************************************










